////////////////////////////////////////////////////////////////////////////////
/// @file TlvMessage.cc
/// @brief 
///
/// Related Files: The definitions are in TlvMessage.h.  
/// Other related files are: 
///
/// Maintained by: LTE eNB Callp Application Team
///
/// Copyright(c) 2009-2010 Nanocell, All Rights Reserved
///
/// CONFIDENTIALITY AND RESTRICTED USE
///
/// This software, including any software of third parties embodied herein,
/// contains information and concepts which are confidential to Nanocell 
/// and such third parties. This software is licensed for use
/// solely in accordance with the terms and conditions of the applicable
/// license agreement with Nanocell or its authorized distributor.
////////////////////////////////////////////////////////////////////////////////

#include <string>
#include <string.h>
#include <netinet/in.h>
#include "TlvMessage.h"

uint64_t  htonl64(uint64_t   host)   
{   
    uint64_t ret = 0;
    uint32_t high = 0;
    uint32_t low = 0;

    low = host & 0xFFFFFFFF;
    high = (host >> 32) & 0xFFFFFFFF;
    
    low = htonl(low);   
    high = htonl(high);   

    ret = low;
    ret <<= 32;   
    ret |= high;   

    return ret;
} 

uint64_t  ntohl64(uint64_t   host)   
{   
    uint64_t ret = 0;   
    uint32_t high = 0;
    uint32_t low = 0;

    low = host & 0xFFFFFFFF;
    high = (host >> 32) & 0xFFFFFFFF;

    low = ntohl(low);   
    high = ntohl(high);   

    ret = low;
    ret <<= 32;   
    ret |= high;   

    return ret;
}


TlvMessage::TlvMessage():
buffer_(0),
bufferLength_(0),
endOfMessage_(0)
{
    AllocBytes(HEADER_LENGTH);
    SetMsgLength(0);
}

TlvMessage::TlvMessage(const Protocol_t protocolType, const Signal_t signalType):
buffer_(0),
bufferLength_(0),
endOfMessage_(0)
{
    AllocBytes(HEADER_LENGTH);
    setProtocolType(protocolType);
    setSignalType(signalType);
    SetMsgLength(0);
}

TlvMessage::TlvMessage(const uint8_t* msgBuffer, const uint16_t length):
buffer_(0),
bufferLength_(0),
endOfMessage_(0)
{
    AllocBytes(length);
    memcpy(&buffer_[0],&msgBuffer[0],length);
    endOfMessage_ = &buffer_[0] + length - 1;
}

TlvMessage::~TlvMessage()
{
    if (buffer_ != 0)
    {
        delete [] buffer_;
        buffer_=0;
    }
}

TlvMessage::Protocol_t TlvMessage::getProtocolType() const
{
    return buffer_[0];
}

void TlvMessage::setProtocolType(const TlvMessage::Protocol_t protocolType)
{
    buffer_[0] = protocolType;
}

TlvMessage::Signal_t TlvMessage::getSignalType() const
{
    return buffer_[1];
}

void TlvMessage::setSignalType(const TlvMessage::Signal_t signalType)
{
    buffer_[1] = signalType;
}

void TlvMessage::getMsgBuffer(uint8_t*& msgBuffer, uint32_t& msgLength)
{
    msgBuffer = buffer_;
    msgLength = ntohs(*(reinterpret_cast<TlvMessage::Length_t*>(&buffer_[2]))) + HEADER_LENGTH;
}

void TlvMessage::addTlv( const TlvTag_t tag, const int8_t value )
{
    int32_t messageIncrease = PARM_HEADER_LENGTH + sizeof(int8_t);
    AllocBytes(messageIncrease);
    endOfMessage_[0] = tag;
    uint16_t size = htons(sizeof(int8_t));
    memcpy(&endOfMessage_[1], &size, sizeof(TlvLength_t));
    endOfMessage_[3] = value;

    AddToMsgLength(messageIncrease);
}
    
bool TlvMessage::getTlv( const TlvTag_t tag, int8_t& value, const int16_t instance )
{
    uint8_t* tlvStart = FindTlv(tag,instance);
    if ( tlvStart != 0 )
    {
        value = tlvStart[3];
    }
    return tlvStart;
}

void TlvMessage::addTlv( const TlvTag_t tag, const uint8_t value )
{
    int32_t messageIncrease = PARM_HEADER_LENGTH + sizeof(uint8_t);
    AllocBytes(messageIncrease);
    endOfMessage_[0] = tag;
    uint16_t size = htons(sizeof(uint8_t));
    memcpy(&endOfMessage_[1], &size, sizeof(TlvLength_t));
    endOfMessage_[3] = value;

    AddToMsgLength(messageIncrease);
}
    
bool TlvMessage::getTlv( const TlvTag_t tag, uint8_t& value, const int16_t instance )
{
    uint8_t* tlvStart = FindTlv(tag,instance);
    if ( tlvStart != 0 )
    {
        value = tlvStart[3];
    }
    return tlvStart;
}

void TlvMessage::addTlv( const TlvTag_t tag, const int16_t value )
{    
    int32_t messageIncrease = PARM_HEADER_LENGTH + sizeof(int16_t);
    AllocBytes(messageIncrease);
    endOfMessage_[0] = tag;
    uint16_t networkSize = htons(sizeof(int16_t));
    memcpy(&endOfMessage_[1], &networkSize, sizeof(TlvLength_t));
    int16_t networkValue = htons(value);
    memcpy( &endOfMessage_[3], &networkValue, sizeof(networkValue) );

    AddToMsgLength(messageIncrease);
}

bool TlvMessage::getTlv( const TlvTag_t tag, int16_t& value, const int16_t instance )
{
    uint8_t* tlvStart = FindTlv(tag,instance);
    if ( tlvStart != 0 )
    {
        int16_t networkValue;
        memcpy( &networkValue, &tlvStart[3], sizeof(networkValue) );
        value = ntohs(networkValue);
    }
    return tlvStart;
}

void TlvMessage::addTlv( const TlvTag_t tag, const uint16_t value )
{    
    int32_t messageIncrease = PARM_HEADER_LENGTH + sizeof(uint16_t);
    AllocBytes(messageIncrease);
    endOfMessage_[0] = tag;
    uint16_t size = htons(sizeof(uint16_t));
    memcpy(&endOfMessage_[1], &size, sizeof(TlvLength_t));
    uint16_t networkValue = htons(value);
    memcpy( &endOfMessage_[3], &networkValue, sizeof(networkValue) );

    AddToMsgLength(messageIncrease);
}

bool TlvMessage::getTlv( const TlvTag_t tag, uint16_t& value, const int16_t instance )
{
    uint8_t* tlvStart = FindTlv(tag,instance);
    if ( tlvStart != 0 )
    {
        uint16_t networkValue;
        memcpy( &networkValue, &tlvStart[3], sizeof(networkValue) );
        value = ntohs(networkValue);
    }
    return tlvStart;
}

void TlvMessage::addTlv( const TlvTag_t tag, const int32_t value )
{    
    int32_t messageIncrease = PARM_HEADER_LENGTH + sizeof(int32_t);
    AllocBytes(messageIncrease);
    endOfMessage_[0] = tag;
    uint16_t size = htons(sizeof(int32_t));
    memcpy(&endOfMessage_[1], &size, sizeof(TlvLength_t));
    int32_t networkValue = htonl(value);
    memcpy( &endOfMessage_[3], &networkValue, sizeof(networkValue) );

    AddToMsgLength(messageIncrease);
}

bool TlvMessage::getTlv( const TlvTag_t tag, int32_t& value, const int16_t instance )
{
    uint8_t* tlvStart = FindTlv(tag,instance);
    if ( tlvStart != 0 )
    {
        int32_t networkValue;
        memcpy( &networkValue, &tlvStart[3], sizeof(networkValue) );
        value = ntohl(networkValue);
    }
    return tlvStart;
}

void TlvMessage::addTlv( const TlvTag_t tag, const uint32_t value )
{    
    int32_t messageIncrease = PARM_HEADER_LENGTH + sizeof(uint32_t);
    AllocBytes(messageIncrease);
    endOfMessage_[0] = tag;
    uint16_t size = htons(sizeof(uint32_t));
    memcpy(&endOfMessage_[1], &size, sizeof(TlvLength_t));
    int32_t networkValue = htonl(value);
    memcpy( &endOfMessage_[3], &networkValue, sizeof(networkValue) );

    AddToMsgLength(messageIncrease);
}

bool TlvMessage::getTlv( const TlvTag_t tag, uint32_t& value, const int16_t instance )
{
    uint8_t* tlvStart = FindTlv(tag,instance);
    if ( tlvStart != 0 )
    {
        uint32_t networkValue;
        memcpy( &networkValue, &tlvStart[3], sizeof(networkValue) );
        value = ntohl(networkValue);
    }
    return tlvStart;
}

void TlvMessage::addTlv(const TlvTag_t tag, const uint64_t value)
{
    int32_t messageIncrease = PARM_HEADER_LENGTH + sizeof(uint64_t);
    AllocBytes(messageIncrease);
    endOfMessage_[0] = tag;
    uint16_t size = htons(sizeof(uint64_t));
    memcpy(&endOfMessage_[1], &size, sizeof(TlvLength_t));
    uint64_t networkValue = htonl64(value);
    memcpy( &endOfMessage_[3], &networkValue, sizeof(networkValue) );

    AddToMsgLength(messageIncrease);
}

bool TlvMessage::getTlv(const TlvTag_t tag, uint64_t& value)
{
    uint8_t* tlvStart = FindTlv(tag,1);
    if ( tlvStart != 0 )
    {
        uint64_t networkValue;
        memcpy( &networkValue, &tlvStart[3], sizeof(networkValue) );
        value = ntohl64(networkValue);
    }
    return tlvStart;
}

void TlvMessage::addTlv( const TlvTag_t tag, const uint8_t* value, const TlvLength_t length )
{    
    int32_t messageIncrease = PARM_HEADER_LENGTH + length;
    AllocBytes(messageIncrease);
    endOfMessage_[0] = tag;
    uint16_t networkLength = htons(length);
    memcpy(&endOfMessage_[1], &networkLength, sizeof(TlvLength_t));
    memcpy( &endOfMessage_[3], value, length );
    AddToMsgLength(messageIncrease);
}

bool TlvMessage::getTlv( const TlvTag_t tag, uint8_t*& value, TlvLength_t& length, const int16_t instance )
{
    uint8_t* tlvStart = FindTlv(tag,instance);
    if ( tlvStart != 0 )
    {
        memcpy(&length, &tlvStart[1], sizeof(TlvLength_t));
        length = ntohs(length);
        ///@todo revisit. This can be made more efficient via the use of 
        ///      a pool of buffers for such a purpose.
        value = new uint8_t[length];
        memcpy( value, &tlvStart[3], length );
    }
    return tlvStart;
}

void TlvMessage::addTlv( const TlvTag_t tag, const uint16_t* value, const TlvLength_t length )
{
    TlvLength_t bytesLength = length*sizeof(uint16_t);
    int32_t messageIncrease = PARM_HEADER_LENGTH + bytesLength;
    AllocBytes(messageIncrease);
    endOfMessage_[0] = tag;
    uint16_t networkLength = htons(bytesLength);
    memcpy(&endOfMessage_[1], &networkLength, sizeof(TlvLength_t));
    for(uint16_t i=0; i<length; i++)
    {
        uint16_t networkValue = htons(*(value+i));
        memcpy( ((uint16_t*)&endOfMessage_[3])+i, &networkValue, sizeof(uint16_t) );
    }
    AddToMsgLength(messageIncrease);
}

bool TlvMessage::getTlv( const TlvTag_t tag, uint16_t*& value, TlvLength_t& length, const int16_t instance )
{    
    TlvLength_t bytesLength = 0;
    uint8_t* tlvStart = FindTlv(tag,instance);
    if ( tlvStart != 0 )
    {
        memcpy(&bytesLength, &tlvStart[1], sizeof(TlvLength_t));
        length = ntohs(bytesLength)/sizeof(uint16_t);
        ///@todo revisit. This can be made more efficient via the use of 
        ///      a pool of buffers for such a purpose.
        value = new uint16_t[length];
        if( value==NULL )
        {
            return false;
        }
        for(uint16_t i=0; i<length; i++)
        {
            uint16_t hostValue = ntohs( *(((uint16_t*)(&tlvStart[3])) + i) );
            *(value+i) = hostValue;
        }
    }
    return tlvStart;
}

void TlvMessage::addTlv(const TlvTag_t tag, const uint32_t* value, const TlvLength_t length)
{
    TlvLength_t bytesLength = length * sizeof(uint32_t);
    int32_t messageIncrease = PARM_HEADER_LENGTH + bytesLength;
    AllocBytes(messageIncrease);
    endOfMessage_[0] = tag;
    uint16_t networkLength = htons(bytesLength);
    memcpy(&endOfMessage_[1], &networkLength, sizeof(TlvLength_t));
    for(uint16_t i=0; i<length; i++)
    {
        uint32_t networkValue = htonl(*(value+i));
        memcpy( ((uint32_t*)&endOfMessage_[3])+i, &networkValue, sizeof(uint32_t) );
    }
    AddToMsgLength(messageIncrease);
}

bool TlvMessage::getTlv(const TlvTag_t tag, uint32_t*& value, TlvLength_t& length, const int16_t instance)
{
    TlvLength_t bytesLength = 0;
    uint8_t* tlvStart = FindTlv(tag,instance);
    if ( tlvStart != 0 )
    {
        memcpy(&bytesLength, &tlvStart[1], sizeof(TlvLength_t));
        length = ntohs(bytesLength)/sizeof(uint32_t);
        ///@todo revisit. This can be made more efficient via the use of 
        ///      a pool of buffers for such a purpose.
        value = new uint32_t[length];
        if( value==NULL )
        {
            return false;
        }
        for(uint16_t i=0; i<length; i++)
        {
            uint32_t hostValue = ntohl( *(((uint32_t*)(&tlvStart[3])) + i) );
            *(value+i) = hostValue;
        }
    }
    return tlvStart;
}

void TlvMessage::AllocBytes(const int32_t additionalBytes)
{
    uint16_t currentLength = endOfMessage_ - buffer_;
    
    if ((currentLength + additionalBytes) >= bufferLength_)
    {
        int32_t newBufferLength = bufferLength_;
        if ( additionalBytes < BUFFER_INCREMENT_LENGTH )
        {
            newBufferLength += BUFFER_INCREMENT_LENGTH;
        }
        else
        {
            newBufferLength += additionalBytes;
        }
        
        uint8_t* newBuffer = new uint8_t[newBufferLength];
        if (newBuffer != 0)
        {
            // copy the old buffer into new one and clear out the new space
            memset(&newBuffer[bufferLength_], 0, newBufferLength-bufferLength_);
            memcpy(&newBuffer[0], &buffer_[0], bufferLength_);
            delete [] buffer_;
        }
        buffer_ = newBuffer;
        bufferLength_ = newBufferLength;
        endOfMessage_ = buffer_ + currentLength;
    }
}
    
void TlvMessage::SetMsgLength(const Length_t length)
{
    uint16_t networkLength = htons(length);
    memcpy(&buffer_[2],&networkLength,sizeof(uint16_t));
    
    endOfMessage_ = buffer_ + HEADER_LENGTH + length;
}
    
void TlvMessage::AddToMsgLength(const Length_t length)
{
    uint16_t newLength1, newLength2, newLength3;
    memcpy(&newLength1,&buffer_[2],sizeof(uint16_t));
    newLength2 = ntohs(newLength1);
    newLength3 = htons(newLength2 + length);
    memcpy(&buffer_[2],&newLength3,sizeof(uint16_t));
    
    endOfMessage_ += length;
}  

uint8_t* TlvMessage::FindTlv(const TlvTag_t tag, const int16_t instance)
{
    uint8_t* tlvMatch = 0;
    uint8_t* currentTlv = &buffer_[HEADER_LENGTH];
    int16_t  instancesToSkip = instance - 1;
    
    while (!tlvMatch && (currentTlv <= endOfMessage_))
    {
        if ( currentTlv[0] == tag )
        {
            if ( instancesToSkip )
            {
                instancesToSkip--;
            }
            else
            {
                tlvMatch = currentTlv;
            }
        }
        
        if ( !tlvMatch )
        {
            uint16_t currentTlvLength;
            memcpy(&currentTlvLength, &currentTlv[1], sizeof(TlvLength_t));
            
            currentTlv += PARM_HEADER_LENGTH + ntohs(currentTlvLength);
        }
    }
    return tlvMatch;
}
    
